/* main.c */
#include "app_inc.h"

/* 全局变量 */
MPU9250_SensorData_T gAppMpu9250SensorDataStruct; /* 存放MPU9250传感器的原始数据 */
KalmanFilter_InputPosData_T gAppKalmanPosData;      /* Kalman滤波器的传入参数 */
extern KalmanFilter_InputPosData_T gAppKalmanPosDataOffset;/* Kalman滤波器的传入偏移量 */
char gAppBleTxInfoBuf[50]; /* 通过BLE向App发送信息时，格式化打印的临时存储器 */
extern uint32_t RgbLeftData[BSP_WS2812B_LED_COUNT], RgbRightData[BSP_WS2812B_LED_COUNT]; /* RGBLED显示数据缓存 */


#if 1 /* 电机调试专用 */

#if 0
Motor_EncCntVal_T gAppMotorEncCntVal =
{
    .LeftEncCntVal = 0,
    .RightEncCntVal = 0
};
#endif

volatile float TargetSpeed=0.0f; /* -90 ~ +90 */
volatile float TargetTurn=0.0f;
volatile float SpeedPwmOut=0.0f, AnglePwmOut=0.0f, TurnPwmOut=0.0f;
volatile bool  DisplayBattery;

#endif

/* 函数声明 */
void App_InitMpu9250Sensor(void);
void App_ProcessMpu9250SensorData(void);
void App_InitMotorControl(void);
void LPITx_Configuration(void);
void LPITx_Start(void);
void ADCx_Configuration(void);
uint32_t ADCx_GetBattMonitorVoltMv(void);
uint32_t ADCx_GetBattMonitorPercentage(void);
void LPTMRx_Configuration(void);
void NVIC_Configuration(void);
void Baterry_And_Turn_Light_Show(uint32_t BatteryPercentage, bool MotorStopFlag, bool SpeedDownFlag);

int main(void)
{
    BSP_InitSystem();   
    BSP_InitStdioLPUART(115200U);
    NVIC_Configuration(); /* 配置中断任务的优先级 */

    printf("\r\n\r\nAllInOne_v1.\r\n");
    printf("Compiled on %s, %s\r\n\r\n", __DATE__, __TIME__);
    printf("Initializing...\r\n");

    App_InitMpu9250Sensor();
    printf("App_InitMpu9250Sensor() Done.\r\n");

    LPITx_Configuration();
    printf("LPITx_Configuration() Done.\r\n");

    ADCx_Configuration();
    printf("ADCx_Configuration() Done.\r\n");

    LPTMRx_Configuration();
    printf("LPTMRx_Configuration() Done.\r\n");

    WS2812B_Init();
    WS2812B_ResetRight();
    WS2812B_ResetLeft();
    WS2812B_DoUpdateColorRight(WS2812B_COLOR_NON);
    WS2812B_DoUpdateColorLeft(WS2812B_COLOR_NON);
    printf("WS2812B_Init() Done.\r\n");

    BleXfer_Init();
    BSP_InitGpioForBLE();     /* GPIO - BLE. */
    /* 配置WAKEUP为低电平，保持唤醒状态 */
    BSP_InitGpioForBLE();     /* GPIO - BLE. */
    /* 配置WAKEUP为低电平，保持唤醒状态 */
    GPIO_SetPinLogic(BSP_GPIO_BLE_WAKEUP_PORT, BSP_GPIO_BLE_WAKEUP_PIN, eGPIO_LogicLevel_Low);
    GPIO_SetPinDir(BSP_GPIO_BLE_WAKEUP_PORT, BSP_GPIO_BLE_WAKEUP_PIN, eGPIO_Direction_Output);
    /* 配置DISCON为低电平，保持断开状态 */
    GPIO_SetPinLogic(BSP_GPIO_BLE_DISCON_PORT, BSP_GPIO_BLE_DISCON_PIN, eGPIO_LogicLevel_Low);
    GPIO_SetPinDir(BSP_GPIO_BLE_DISCON_PORT, BSP_GPIO_BLE_DISCON_PIN, eGPIO_Direction_Output);   
    printf("BleXfer_Init() Done.\r\n");

    App_InitMotorControl();
    printf("App_InitMotorControl() Done.\r\n");

    printf("All Ready.\r\n");

    /* 启用周期采样 */
    LPITx_Start();
    LPTMR_HAL_Start();

    /* 上电时使用RGBLED显示电量百分比 */
	DisplayBattery = true;

    while (1)
    {
        switch (BleXfer_GetRunCmdFromCmdFifo())
        {
            case 'A': /* 切换至向前，左右轮均向前 */
                printf("A");
//                WS2812B_DoUpdateColorLeft(WS2812B_COLOR_GREEN);
//                WS2812B_DoUpdateColorRight(WS2812B_COLOR_GREEN);
                TargetSpeed = -60.0f; /* 设定电机前进 */
                break;
            case 'B': /* 切换至向前向右，左轮向前，右轮不变 */
                printf("B");
                printf("AdcValue: %d\r\n", (int)GlobalAppInfoStruct.BattMonitorVoltageAdcValue);
                break;
            case 'C': /* 切换至原地向右，左轮向前，右轮向后，左右转速相同，方向相反 */
                printf("C");
//                WS2812B_DoUpdateColorLeft(WS2812B_COLOR_GREEN);
//                WS2812B_DoUpdateColorRight(WS2812B_COLOR_RED);
                TargetTurn = -50.0f;
                break;
            case 'D': /* 切换至：左轮不变，右轮向后 */
//                WS2812B_DoUpdateColorLeft(WS2812B_COLOR_RED | WS2812B_COLOR_GREEN | WS2812B_COLOR_BLUE);
//                WS2812B_DoUpdateColorRight(WS2812B_COLOR_RED | WS2812B_COLOR_GREEN | WS2812B_COLOR_BLUE);
                printf("D");
                break;
            case 'E': /* 切换至向后，左右轮均向后 */
                printf("E");
                WS2812B_DoUpdateColorLeft(WS2812B_COLOR_RED);
                WS2812B_DoUpdateColorRight(WS2812B_COLOR_RED);
                TargetSpeed = 60.0f; /* 设定电机后退 */
                break;
            case 'F': /* 切换至：左轮向后，右轮向左 */
                printf("F");
                break;
            case 'G': /* 切换至原地向左，左轮向后，右轮向前 */
                printf("G");
//                WS2812B_DoUpdateColorLeft(WS2812B_COLOR_RED);
//                WS2812B_DoUpdateColorRight(WS2812B_COLOR_GREEN);
                TargetTurn = 50.0f;
                break;
            case 'H': /* 切换至向前向左，左轮不变，右轮向前 */
                printf("H");
                break;
            case 'Z': /* 切换至原地待命 */
                printf("Z");
//                WS2812B_DoUpdateColorLeft(0x00);
//                WS2812B_DoUpdateColorRight(0x00);
                TargetSpeed = 0.0f; /* 设定电机在当前位置平衡不动 */
                TargetTurn = 0.0f;
                break;
            case 'X': /* 切换至高速挡 */
                break;
            case 'Y': /* 切换至低速挡 */
                break;
            default: /* 保持现状 */
                break;
        }        
    }
}


/******************************************************************************
 * MPU9250.
 *****************************************************************************/

void App_InitMpu9250Sensor(void)
{
    /* 初始化LPSPI_EDMA传输 */
    MPU9250_Init();
    printf("MPU9250_Init() Done.\r\n");

    /* MPU9250芯片的上电时间比较久，等待MPU9250启动之后再对其进行初始化 */
    while (!MPU9250_Probe())
    {
        printf("MPU9250_Probe() Failed.\r\n");
    }
    printf("MPU9250_Probe() Done.\r\n");

    /* 配置MPU9250的工作方式，转换完成后产生中断 */
    MPU9250_Setup();
}

/* 初始化电机为待命状态 */
void App_InitMotorControl(void)
{
    BSP_InitMotorTurn();
    BSP_InitMotorQuadDecoder();
    Motor_InitDevice();
    Motor_EnableTurn(true);
}

/* 获取传感器数据之后的处理流程 */
void App_ProcessMpu9250SensorData(void)
{
    float KalmanAngle;
	static bool MotorStopFlag=false, PickUpFlag=false, SpeedDownFlag=false;
	static uint32_t BatteryPercentage; 
    static uint32_t CurSpeed, PreSpeed;
    static int SpeedDiff;
    Motor_EncCntVal_T MotorEncCntVal =
    {
        .LeftEncCntVal = 0,
        .RightEncCntVal = 0
    };
    Motor_EncCntVal_T MotorEncCntValPre =
    {
        .LeftEncCntVal = 0,
        .RightEncCntVal = 0
    };

    /* 格式化传感器数据 */
    MPU9250_GetSensorData(&gAppMpu9250SensorDataStruct);

    /* 使用Kalman滤波器处理，得到滤波后的倾角 */
    gAppKalmanPosData.AccelForwardVal = -gAppMpu9250SensorDataStruct.AccY;
    gAppKalmanPosData.AccelDownVal = gAppMpu9250SensorDataStruct.AccZ;
    gAppKalmanPosData.GyroForwardVal = -gAppMpu9250SensorDataStruct.GyrX;
    KalmanAngle = KalmanFilter_Process(&gAppKalmanPosData, &gAppKalmanPosDataOffset);

    /* 注册滤波得到的角度到注册信息表中 */
    GlobalAppInfoStruct.AngleValue = 1000 * (int)KalmanAngle;

    /* Important.
     * 使用倾角，通过PID算法，计算电机的调整值
     */
    /* 计算角度环PWM控制量 */
    AnglePwmOut = Motor_AnglePwmControl(KalmanAngle, gAppKalmanPosData.GyroForwardVal);

    /* 保存前一次速度值 */
    MotorEncCntValPre = MotorEncCntVal;
    
    /* 获取速度编码器在一个控制周期内的累积量 */
    Motor_GetSpeedPulse(&MotorEncCntVal);

    /* 注册当前转速信息到注册信息表中 */
    GlobalAppInfoStruct.MotorLeftEncoderValue = MotorEncCntVal.LeftEncCntVal;
    GlobalAppInfoStruct.MotorRightEncoderValue = MotorEncCntVal.RightEncCntVal;
		
    /* 计算速度环PWM控制量 */
    SpeedPwmOut = Motor_SpeedPwmControl(&MotorEncCntVal, TargetSpeed, MotorStopFlag);

    /* 计算转向PWM控制量 */
    TurnPwmOut = Motor_TurnPwmControl(TargetTurn, gAppMpu9250SensorDataStruct.GyrZ);
		
	/* 检测小车是否被拿起 */
	if(Car_DetectPickUp(gAppMpu9250SensorDataStruct.AccZ, KalmanAngle, &MotorEncCntVal) == true) {
		PickUpFlag = true;
	}

	/* 检测小车是否被放下 */		
	if(Car_DetectPutDown(MotorStopFlag, KalmanAngle, &MotorEncCntVal) == true) {
		PickUpFlag = false;
	}

	/* 获取电池电量百分比 */		
	BatteryPercentage = ADCx_GetBattMonitorPercentage();
		
	/* 检测小车倾角是否大于60度，电池电量是否低于30%，是否被拿起。任一条件满足，都会强制关闭电机 */			
	MotorStopFlag = Motor_Stop(KalmanAngle, BatteryPercentage,PickUpFlag);
				
    /* 如果电机不处于强制关闭状态，则更新PWM控制器 */
	if(MotorStopFlag == false ) {
		Motor_RefreshPwmController(AnglePwmOut, SpeedPwmOut, TurnPwmOut);
	}
		
    CurSpeed = abs(MotorEncCntVal.LeftEncCntVal + MotorEncCntVal.RightEncCntVal);
    PreSpeed = abs(MotorEncCntValPre.LeftEncCntVal + MotorEncCntValPre.RightEncCntVal);
    SpeedDiff = PreSpeed - CurSpeed;
    if( SpeedDiff > 5 )
    {
        SpeedDownFlag = true;
    }
    else
    {
        SpeedDownFlag = false;        
    }
    /* 上电时使用RGBLED显示电量百分比, 总共6个灯，每个灯代表17%的电量 */		
    /* 高于50%，显示绿色；30%-50%，显示橙色；低于30%，显示红色 */
    /* 闪烁六次后，切换回转向灯功能 */
//    BatteryPercentage = 40;
    Baterry_And_Turn_Light_Show(BatteryPercentage, MotorStopFlag, SpeedDownFlag);
}

/******************************************************************************
 * ADCx.
 *****************************************************************************/

/*
* 初始化ADC模块
* 1. 设定检测电池电压的采样通道
* 2. 启用连续转换模式
*/
void ADCx_Configuration(void)
{
    ADC_ConverterConfig_T adcConverterConfigStruct;
    ADC_ConvCommand_T adcConvCommandStruct;

    /* 配置ADC引脚及转换时钟源 */
    BSP_InitAdcBatterySensor();

    /* 配置转换器 */
    adcConverterConfigStruct.RefVoltSource          = eADC_RefVoltSourceSel_Vref;
    adcConverterConfigStruct.ClockSource            = eADC_ClockSourceSel_Alt0;
    adcConverterConfigStruct.ClockDivider           = eADC_ClockDividerSel_8;
    adcConverterConfigStruct.ResolutionBits         = eADC_ResolutionBits_12; /* 12-bit采样精度 */
    adcConverterConfigStruct.SampleClockCount       = 0x3F;
    ADC_HAL_ConfigConverter(BSP_ADC_BATTERY_SENSOR_PORT, &adcConverterConfigStruct);
    ADC_HAL_SetConvTriggerMode(BSP_ADC_BATTERY_SENSOR_PORT, eADC_ConvTriggerBySoftware); /* 软件触发 */
    ADC_HAL_SetHardwareAverageMode(BSP_ADC_BATTERY_SENSOR_PORT, eADC_HardwareAverageCount32); /* 启用硬件平均 */
    ADC_HAL_DoAutoCalibration(BSP_ADC_BATTERY_SENSOR_PORT); /* 开始校准 */
    ADC_HAL_EnableContinuousConvMode(BSP_ADC_BATTERY_SENSOR_PORT, true); /* 连续转换模式 */

    /* 配置转换通道 */
    adcConvCommandStruct.ConvChannelNum = BSP_ADC_BATTERY_SENSOR_CAHNNEL_INDEX;
    adcConvCommandStruct.enInterruptOnCovnDone = false;
    ADC_HAL_SetChannelConvCmd(BSP_ADC_BATTERY_SENSOR_PORT, BSP_ADC_BATTERY_SENSOR_CAHNNEL_GROUP, &adcConvCommandStruct);
}

/*
* 获取电池电量百分比
* 1. 从ADC读数并触发下一次采样
* 2. 换算成电量的百分比
*/
//#define APP_BATTERY_VOLTAGE_MIN_MV  1183U
//#define APP_BATTERY_VOLTAGE_MAX_MV  3127U
#define APP_BATTERY_VOLTAGE_MIN_MV  2704U
#define APP_BATTERY_VOLTAGE_MAX_MV  3549U
uint32_t ADCx_GetBattMonitorPercentage(void)
{
    uint32_t tmp32;

    tmp32 = ADC_HAL_GetChannelConvValue(BSP_ADC_BATTERY_SENSOR_PORT, BSP_ADC_BATTERY_SENSOR_CAHNNEL_GROUP);
    GlobalAppInfoStruct.BattMonitorVoltageAdcValue = tmp32;
    /* 首先将采样数值换算成采样电压，参考电压为3300mV. */
    tmp32 = (tmp32 * 3300U) >>12;

//    /*
//    * 将采样电压在电池电压的安全区间内换算成百分比
//    * 3S的锂电池充满电为11.1v，最低安全电压是4.2V，经过510K和200K的电阻分压，测试点对应充满电的电压为3127mV/1183mV */
//    if (tmp32 <= APP_BATTERY_VOLTAGE_MIN_MV)
//    {
//        tmp32 = 1U;
//    }
//    else if (tmp32 >= APP_BATTERY_VOLTAGE_MAX_MV)
//    {
//        tmp32 = 100U;
//    }
//    else
//    {
//        tmp32 = (tmp32 - 1183U)*100U/(3127U-1183U);
//    }

    /*
    * 将采样电压在电池电压的安全区间内换算成百分比
    * 3S的锂电池充满电为12.6v，放电截止电压是9.6V，经过510K和200K的电阻分压，测试点对应充满电和放电截止的电压为3549mV/2704mV */
    if (tmp32 <= APP_BATTERY_VOLTAGE_MIN_MV)
    {
        tmp32 = 1U;
    }
    else if (tmp32 >= APP_BATTERY_VOLTAGE_MAX_MV)
    {
        tmp32 = 100U;
    }
    else
    {
        tmp32 = (tmp32 - APP_BATTERY_VOLTAGE_MIN_MV)*100U/(APP_BATTERY_VOLTAGE_MAX_MV-APP_BATTERY_VOLTAGE_MIN_MV);
    }

    return tmp32;
}

/******************************************************************************
 * LPITx.
 *****************************************************************************/
/*
* 初始化LPIT模块
* 1. 配置通道0周期触发姿态传感器更新数据，计算电机的调校值
* 2. 其余周期触发事件交给LPTMR完成
* 3. 中断优先级高于其余的定时器中断
*/
void LPITx_Configuration(void)
{
    LPIT_ChannelConfig_T lpitChannelConfigStruct;

    /* 启用LPIT0模块时钟 */
    PCC->CLKCFG[PCC_LPIT0_INDEX] = PCC_CLKCFG_CGC_MASK | PCC_CLKCFG_PCS(6U); /* BSP_CLK_SPLLDIV2_HZ. */

    /* 配置计数器 */
    LPIT_HAL_Enable(LPIT0, true);
    LPIT_HAL_Reset(LPIT0);
    LPIT_HAL_GetChannelConfigForNormalTimer(&lpitChannelConfigStruct);

    /* 配置姿态传感器的周期触发通道 */
    LPIT_HAL_ConfigChannel(LPIT0, APP_LPIT_MPU9250_CHANNEL, &lpitChannelConfigStruct);
    LPTI_HAL_SetChannelTimeoutValue(LPIT0, APP_LPIT_MPU9250_CHANNEL, BSP_CLK_SPLLDIV2_HZ/100U); /* 指定PIT的中断周期，100Hz, 10ms */
    LPIT_HAL_EnableChannelInterrupt(LPIT0, APP_LPIT_MPU9250_CHANNEL, true);
#if 0
    /* 配置电池电压监控的周期触发通道 */
    LPIT_HAL_ConfigChannel(LPIT0, APP_LPIT_BATTERY_CHANNEL, &lpitChannelConfigStruct);
    LPTI_HAL_SetChannelTimeoutValue(LPIT0, APP_LPIT_BATTERY_CHANNEL, BSP_CLK_SPLLDIV2_HZ); /* 指定PIT的中断周期，1Hz, 1s */
    LPIT_HAL_EnableChannelInterrupt(LPIT0, APP_LPIT_BATTERY_CHANNEL, true);

    /* 配置向BLE的App软件同步信息的周期触发通道 */
    LPIT_HAL_ConfigChannel(LPIT0, APP_LPIT_BLE_TX_INFO_CHANNEL, &lpitChannelConfigStruct);
    LPTI_HAL_SetChannelTimeoutValue(LPIT0, APP_LPIT_BLE_TX_INFO_CHANNEL, BSP_CLK_SPLLDIV2_HZ); /* 指定PIT的中断周期，1Hz, 1s */
    LPIT_HAL_EnableChannelInterrupt(LPIT0, APP_LPIT_BLE_TX_INFO_CHANNEL, true);
#endif

    /* 配置控制EDMA周期翻转GPIO用于产生RGBLED控制信号的触发通道 */
    LPIT_HAL_ConfigChannel(LPIT0, APP_LPIT_RGB_CONTROL_CHANNEL, &lpitChannelConfigStruct);
//    LPTI_HAL_SetChannelTimeoutValue(LPIT0, APP_LPIT_RGB_CONTROL_CHANNEL, BSP_CLK_SPLLDIV2_HZ/20000000U); /* 指定PIT的中断周期，20MHz, 0.05us */
    LPTI_HAL_SetChannelTimeoutValue(LPIT0, APP_LPIT_RGB_CONTROL_CHANNEL, BSP_CLK_SPLLDIV2_HZ/5000000U); /* 指定PIT的中断周期，5MHz, 0.2us */
    LPIT_HAL_EnableChannelInterrupt(LPIT0, APP_LPIT_RGB_CONTROL_CHANNEL, true);

    /* 解锁NVIC中LPIT的中断向量 */
    NVIC_EnableIRQ(LPIT0_IRQn);
}

/*
* 启动PIT定时器
* 1. 启动电池电压ADC采样定时器
* 2. 启动姿态传感器采样定时器
*/
void LPITx_Start(void)
{
    /* 启用周期采样，开始刷新MPU9250的采样值，并执行响应的处理操作 */
    LPIT_HAL_StartChannel(LPIT0, APP_LPIT_MPU9250_CHANNEL);
#if 0
    /* 启用周期采样，开始刷新电池电压ADC的采样值 */
    LPIT_HAL_StartChannel(LPIT0, APP_LPIT_BATTERY_CHANNEL);

    /* 启用周期触发，开始于App同步状态 */
    LPIT_HAL_StartChannel(LPIT0, APP_LPIT_BLE_TX_INFO_CHANNEL);
#endif
}

/*
* LPIT中断服务程序入口
*/
void LPIT0_IRQHandler(void)
{
    /* 周期采样MPU9250传感器通道 */
    if ( LPIT_HAL_GetChannelTimeoutFlag(LPIT0, APP_LPIT_MPU9250_CHANNEL) )
    {
        LPTI_HAL_ClearChannelTimeoutFlag(LPIT0, APP_LPIT_MPU9250_CHANNEL);
        /* 触发转换 */
        MPU9250_UpdateSensorData(App_ProcessMpu9250SensorData); /* 在转换结束后处理数据 */
    }
#if 0
    /* 周期触发电池电压检测 */
    if ( LPIT_HAL_GetChannelTimeoutFlag(LPIT0, APP_LPIT_BATTERY_CHANNEL) )
    {
        LPTI_HAL_ClearChannelTimeoutFlag(LPIT0, APP_LPIT_BATTERY_CHANNEL);
        /* 读取上次ADC转换的结果，并启用下一次触发 */
        GlobalAppInfoStruct.BattMonitorVoltagePercentage = ADCx_GetBattMonitorPercentage();
    }
    /* 周期触发BLE向App发送消息 */
    if ( LPIT_HAL_GetChannelTimeoutFlag(LPIT0, APP_LPIT_BLE_TX_INFO_CHANNEL) )
    {
        LPTI_HAL_ClearChannelTimeoutFlag(LPIT0, APP_LPIT_BLE_TX_INFO_CHANNEL);

        sprintf(gAppBleTxInfoBuf, "{A%d:%d:%d:%d}$",
            GlobalAppInfoStruct.MotorLeftEncoderValue,
            GlobalAppInfoStruct.MotorRightEncoderValue,
            GlobalAppInfoStruct.BattMonitorVoltagePercentage,
            GlobalAppInfoStruct.AngleValue/1000
        );
        BleXfer_Send((uint8_t *)gAppBleTxInfoBuf, strlen(gAppBleTxInfoBuf), NULL);
    }
#endif  
}


/******************************************************************************
 * LPTMRx.
 *****************************************************************************/
/*
* 配置LPTMR
* 1. 周期触发中断，超时周期为1s
* 2. 在对应的中断程序中搜集注册表中的数据，并通过BLE同远程的App界面同步显示
* 3. 中断优先级低于PIT中断
*/
void LPTMRx_Configuration(void)
{
    LPTMR_PitConfig_T lptmrPitConfigStruct;

    /* 配置LPTMR0时钟源 */
    PCC->CLKCFG[PCC_LPTMR0_INDEX] = PCC_CLKCFG_CGC_MASK | PCC_CLKCFG_PCS(6); /* BSP_CLK_SPLLDIV2_HZ, 84MHz. */

    /* 配置定时器 */
    LPTMR_HAL_Reset();

    lptmrPitConfigStruct.ClockSource = eLPTMR_ClockSource_PCC_Sel; /* 来自PCC的时钟源 */
    lptmrPitConfigStruct.ClockPreDiv = eLPTMR_ClockPreDiv_65536; /* 尽量大的分频 */
    LPTMR_HAL_ConfigPitMode(&lptmrPitConfigStruct);

    /* 设定超时周期为1s */
    LPTMR_HAL_SetTimeoutValue((BSP_CLK_SPLLDIV2_HZ >> 16)/2U); /* 使用eLPTMR_ClockPreDiv_65536分频后的1s,再切分，触发周期为2HZ */

    /* 开中断 */
    NVIC_EnableIRQ(LPTMR0_IRQn);
    LPTMR_HAL_SetIntEnabledOnTimeout(true);
}

/*
* LPTMR0中断服务程序入口
*/
void LPTMR0_IRQHandler(void)
{
    /* 清LPTMR溢出标志 */
    LPTMR_HAL_ClearTimeoutFlag();

    /* 读取上次ADC转换的结果，并启用下一次触发 */
    GlobalAppInfoStruct.BattMonitorVoltagePercentage = ADCx_GetBattMonitorPercentage();

    /* 周期触发BLE向App发送消息 */
    sprintf(gAppBleTxInfoBuf, "{A%d:%d:%d:%d}$",
            (int)GlobalAppInfoStruct.MotorLeftEncoderValue,
            (int)GlobalAppInfoStruct.MotorRightEncoderValue,
            (int)GlobalAppInfoStruct.BattMonitorVoltagePercentage,
            (int)GlobalAppInfoStruct.AngleValue/1000
        );
    BleXfer_Send((uint8_t *)gAppBleTxInfoBuf, strlen(gAppBleTxInfoBuf), NULL);
}

/******************************************************************************
 * NVIC.
 *****************************************************************************/
/*
* 配置NVIC中断管理单元
* 1. 在NVIC中解锁必要的中断向量
* 2. 设定各中断向量的优先级
* 3. 在KE18F中实现了4个有效的优先级位，具体查看 __NVIC_PRIO_BITS.
* 4. 优先级的值越小，级别越高
*/
#define __NVIC_PRIO(preep, sub) ((((preep) & 0x3) << 2) | (sub & 0x3) )
void NVIC_Configuration(void)
{
    NVIC_SetPriorityGrouping(5U); /* 最高2位为可抢占优先级，底2位为不可抢占优先级 */
    NVIC_SetPriority(LPIT0_IRQn,  __NVIC_PRIO(1, 2));//    NVIC_SetPriority(LPIT3_IRQn,  __NVIC_PRIO(1, 2));
    NVIC_SetPriority(LPTMR0_IRQn, __NVIC_PRIO(2, 2));
    NVIC_SetPriority(DMA0_IRQn,   __NVIC_PRIO(3, 2));
    NVIC_SetPriority(DMA1_IRQn,   __NVIC_PRIO(3, 2));
    NVIC_SetPriority(DMA2_IRQn,   __NVIC_PRIO(3, 2));
    NVIC_SetPriority(DMA3_IRQn,   __NVIC_PRIO(1, 2));
}


void Baterry_And_Turn_Light_Show(uint32_t BatteryPercentage, bool MotorStopFlag, bool SpeedDownFlag)
{
    static uint32_t RgbLedCnt=0;
    static bool BaterryBarShow=true;
    static bool RgbLedCntIncFlag=true;
    
    if(DisplayBattery == true ) 
    {
        RgbLedCnt++;
        #if (BSP_WS2812B_LED_COUNT == 3)
        if( BatteryPercentage >= 80 ) 
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*2U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*3U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*4U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*5U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }                                            
            }
            else
            {               
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }
        }
        else if( BatteryPercentage >= 64 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*2U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*3U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*4U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {                               
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }     
            }                
        }
        else if( BatteryPercentage >= 48 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*2U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*3U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }                
        }
        else if( BatteryPercentage >= 32 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_ORANGE);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*2U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }     
            }                    
        } 
        else if( BatteryPercentage >= 16 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }   
            }                    
        } 
        else
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }                    
        } 
        #endif

        #if (BSP_WS2812B_LED_COUNT == 2)
        if( BatteryPercentage >=75) 
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*2U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*3U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {               
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }
        }
        else if( BatteryPercentage >= 50 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U*2U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {                               
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }     
            }                
        }
        else if( BatteryPercentage >= 40 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_GREEN_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }                
        }
        else if( BatteryPercentage >= 25 )
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_ORANGE, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else if( RgbLedCnt == 30U) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }                
        } 
        else
        {
            if( BaterryBarShow == true)
            {
                if( RgbLedCnt == 1) {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                    BaterryBarShow = false;
                    RgbLedCnt = 0;
                }
            }
            else
            {
                if( ((RgbLedCnt/50)%2) == 0 )
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
                else
                {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                }
            }                    
        } 
        #endif
        
        if( (RgbLedCnt == 350U) && (BaterryBarShow == false) ) 
        {
            DisplayBattery = false;
            RgbLedCnt = 0;
        }
    }
    /* 转向时控制RGBLED闪烁 */
    else
    {	
        if(MotorStopFlag == false)
        {
          #if (BSP_WS2812B_LED_COUNT == 3)
          if(TargetTurn == 0.0f) {
              if(TargetSpeed > 0.0f) {
                  Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_WHITE_INTENSITY);
                  Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_WHITE_INTENSITY);
                  Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              } 
              else if(TargetSpeed < 0.0f)
              {
                  if(SpeedDownFlag == false)
                  {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);
                  }
                  else
                  {
                    Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_RED_INTENSITY, WS2812B_COLOR_RED_INTENSITY);
                    Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);                  
                  }
              }
              else
              {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);                  
              }
              RgbLedCnt = 0;
          } else if(TargetTurn < 0.0f) {
              RgbLedCnt++;
              if(RgbLedCnt == 50) {           
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              } else if(RgbLedCnt == 100) {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                RgbLedCnt = 0;
              }
          } else {
              RgbLedCnt++;
              if(RgbLedCnt == 50) {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              } else if(RgbLedCnt == 100) {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                RgbLedCnt = 0;
              }			
          }
          #endif

          #if (BSP_WS2812B_LED_COUNT == 2)
          if(TargetTurn == 0.0f) {
              if(TargetSpeed > 0.0f) {
                  Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_NON);
                  Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_WHITE_INTENSITY, WS2812B_COLOR_NON);
                  Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              } else {
                  Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_NON);
                  Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_BLUE_WEAK, WS2812B_COLOR_NON);
                  Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              }				
              RgbLedCnt = 0;
          } else if(TargetTurn < 0.0f) {
              RgbLedCnt++;
              if(RgbLedCnt == 50) {           
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              } else if(RgbLedCnt == 100) {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                RgbLedCnt = 0;
              }
          } else {
              RgbLedCnt++;
              if(RgbLedCnt == 50) {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_ORANGE, WS2812B_COLOR_ORANGE, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
              } else if(RgbLedCnt == 100) {
                Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_NON, WS2812B_COLOR_NON, WS2812B_COLOR_NON);
                Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
                RgbLedCnt = 0;
              }			
          }
          #endif                  
        }
        else
        {
            Ws2812b_UpdateColorBuf(RgbLeftData, WS2812B_COLOR_BLUE_BREATHE(RgbLedCnt), WS2812B_COLOR_BLUE_BREATHE(RgbLedCnt), WS2812B_COLOR_BLUE_BREATHE(RgbLedCnt));
            Ws2812b_UpdateColorBuf(RgbRightData, WS2812B_COLOR_BLUE_BREATHE(RgbLedCnt), WS2812B_COLOR_BLUE_BREATHE(RgbLedCnt), WS2812B_COLOR_BLUE_BREATHE(RgbLedCnt));
            Ws2812b_GenerateControlTiming(RgbLeftData, RgbRightData);  
            if(RgbLedCnt == 100)
            {
                RgbLedCntIncFlag = false;
            }
            else if(RgbLedCnt == 0)
            {
                RgbLedCntIncFlag = true;               
            }
            
            if(RgbLedCntIncFlag == true)
            {
                RgbLedCnt++;
            }
            else
            {
                RgbLedCnt--;                
            }
        }
    }  
}


/* EOF. */

